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KINEMATIC EQUATIONS FOR RESOLVED-RATE CONTROL OF AN INDUSTRIAL ROBOT ARM 


L. Keith Barker 
SUMMARY 


An operator can use kinematic, resolved-rate equations to dynamically control 
a robot arm by watching its response to commanded inputs. In a tutorial fashion, 
this paper derives known resolved-rate equations for the control of a particular six- 
degree-of-freedom industrial robot arm and proceeds to simplify the equations for 
faster computations. Methods for controlling the robot arm in regions which normally 
cause mathematical singularities in the resolved-rate equations are discussed. 


INTRODUCTION 


In the Intelligent Systems Research Laboratory at the Langley Research Center, 
an operator sits at a remote console with a three-axis controller in each hand and 
commands the motions of an industrial robot arm. The operator has optional control 
modes. In particular, resolved-rate control (ref. l) enables the operator to 
directly control the robot hand. The operator views the robot hand, decides that he 
wants it to move in a certain direction, and deflects a controller. The robot hand 
then moves accordingly with a velocity proportional to the amount of deflection of 
the controller. Commanded hand velocities are transformed (resolved) into requisite 
movements (velocities) of the individual joints in the robot arm to effect the com- 
manded hand motion. 

The intent of this paper is to: (l) derive the resolved-rate equations in 

reference 2 from a point of view of an operator remotely controlling a robot arm; (2) 
simplify these equations for real-time application; (3) leave additional parameters 
unspecified in the final equations for more flexibility in modeling the robot arm; 
and ( 4 ) further document a set of resolved-rate equations. 


SYMBOLS 


A 1 !-! 


Ai, a^ 

CAL 3 

Ci 

C 23 

Dl, D2, D 3 , DU, D 5 


homogeneous transformation matrix from coordinate 
system i to coordinate system i-1 

common normal between Z^_ ^ and Z^ 

Cos C13 

Cos 0 1 

Cos (02 + 63) 

functions defined by equations (52) to (56), respectively 


a/SV" /e 



^k6> ^k,6 


A A 

fLk6» i*k,6 

ES 

FI, F3, F4, F5, F 6, FT 
F2 

£l, £2 

HW 

i 

K2, K3 
k 

M, Ml, M2 
M*, Ml*, M2* 


vector from coordinate system k to hand coordinate 
system 

flk6 in base coordinates 
elbow-to-shoulder length 

functions defined by equations (27), (29), (30), (3l), (4i) 
and (37), respectively 

constant defined by equation ( 28 ) 

vectors defined by equation (42) and (59), respectively 
hand-to-vrist length 

integer which indicates different axis systems and 
associated parameters 

constant gains 

integer 

matrices defined by equations ( 26 ), (43), and (58), 
respectively 

generalized matrix inverse of M, Ml, and M2, 
respectively 


neck-to-base length 

£0k’ J20,k position vector in base coordinates from base 

coordinate system to coordinate system k 


point in cartesian coordinates 


£ 


vector to point Q 


RVEL 


A 

RVEL 


rotational velocity of robot hand, expressed in hand axis 
system 

RVEL in %ase coordinates 


resultant rotational velocity of that commanded and that 
induced by rotations of joints 1, 2, and 3 (eq. 48) 


RVEL(l), RVEL (2), RVEL ( 3 ) components of RVEL 
RVEL(l), RVEL (2), RVEL(3) components of RVEL 
RVEL(l), RVEL (2), RVEL (3) components of RVEL 


R 5 - 


i-1 


2 


rotational transformation matrix from coordinate 
system i to coordinate system i - 1 



Hi, ri 



relative distance between coordinate system i-1 and 
coordinate system i, measured along Zi_i 

SAL 3 



Sin 03 

Si 



Sin 0 i 

S 23 



Sin (02 + 63) 

SN 



shoulder-to-neck length 

SN 



SN + R 3 

TVEL 

A 

TVEL 



translational velocity of robot hand in hand axis 
system 



TVEL in base coordinates 

TVEL(l) 

, TVEL ( 2 ) , 

TVEL( 3 ) 

components of TVEL 

A 

A 

A 

A 

TVEL(l) 

, TVEL ( 2 ) , 

TVEL( 3 ) 

components of TVEL 

At 



time increment 

A 

Vi 



translational velocity of hand axis system 
caused by rotation of joint i, expressed in base 
coordinates 

WE 



wrist-to-elbow length 

*1 



axis directed along common normal between Zi_i and Zj 

*i 



axis directed to complete right-hand axis system with 
and Z-j_ 

Zi 



axis of rotation of joint i-1 

X 0 , Y 0 , 

z 0 


base coordinate system 

X6, y 6 . 

z 6 


hand coordinate system 

iii’ Xi> 

z i 


unit vectors along X^, Y-^, Z^ 

A A 

A 



Zi. 

hx 


unit vectors _>£ , y^ , Zj_ , expressed in base 
coordinates 

a i 



angle between Zj__i and Zj_, measured positive about 

Xi 

®i 



joint angle with initial value corresponding to initial 


position of robot arm in figure 3 


3 



0'^ joint angle between X^_^ and X^, measured positive 

about 

0 vector of joint angles (eq. 12) 

_§New> ®Dld next and previous vector of joint angles, respectively 

to-i rotational velocity of joint i about Zi_]_ 

6J.; uj-j_ expressed in base coordinates 

Use of a dot over a symbol indicates first derivative with respect to time, a 
vector is underlined, and a caret ( A ) above a vector means that the vector is ex- 
pressed with respect to the base coordinate system. 


ANALYSIS 


Figure 1 represents a six-degree-of-freedom industrial manipulator. In refer- 
ence 2, three of these manipulators served as legs in simulating the locomotion of a 
three-legged robot over structural beams. In the current paper, based on reference 
2, an operator controls the motions of a robot arm. 

Controlling individual joints in a robot arm to accomplish a complex task is 
difficult, especially if time to complete the task is critical or if part of the 
operator's attention is needed elsewhere. A more natural approach is for an operator 
to command the motion of the robot hand and then automate the requisite coordination 
of the individual joints in the arm (ref. l). The relative joint geometry dictates 
the basic transformation equations. 


Joint Axis Systems and Transformation Matrices 

Consecutive joint axis systems in robotic manipulators can be related by the 
Denavit-Hartenberg parameters (ref. 3). For rotational joints, (figure 2) these 
parameters consist in three constant parameters a^, r^, and a variable joint 
angle 0^. By definition, joints always rotate about their Z-axis. The Yj__]_- and 
Y^— axis (not shown) complete right-handed coordinate systems. (Although not 
considered here, r-j_ is the variable for prismatic joints.) 

The homogeneous transformation matrix (based on figure 2) from coordinate system 
i to coordinate system i-1 is (refs. 2 or 4, for example) 







1 


n 

cos 

0.' 

l 

-cos a. sin 
l 

0.' 

l 

sin a. sin 
l 

e i j 

a. cos 

l 

CD 

sin 

*■ *H 
CD 

cos a. cos 
i 

*■ *H 
CD 

-sin a. cos 

i 

0 .* i 

i i 

i 

a. sin 
i 

— *H 
CD 

0 


sin a. 

l 


cos a. 

l 

i 

i 

i 

i 

r. 

l 


0 


0 


0 

i 

i 

i 

1 


_• 







— J 
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The parameter ri plays the role of s* in reference 2 and d* in reference 3. 


Let the location of a point Q with respect to the coordinate system (X^, Y^, 
Z^) be described by the vector q^. Then, the location of Q from coordinate 
system Y^_^, Z^_i) is the vector where 



( 2 ) 


in which A*j_j_ accounts for both rotation and displacement of (X^, Y^, Z^) 
with respect to (Xi_^, ^i-1* Z^_i). However, if one is only interested in the 
components of in directions parallel to (X^_i, Y^_^ , Z^^), such as veloc- 
ity, then it is sufficient to compute: 


where 


R Ll 


4i-l = 

R? .q. 
1-I—1 



cos 

CD 

-cos 

a. 

1 

sin 

sin 

0.' 

1 

cos 

a. 

1 

cos 

0 



sin 

CL. 


1 

3.' 

l 


sin a. sin 0/ 

1 1 

-sin a. cos 0/ 

l l 


cos a. 

l 


(3) 


(4) 


is the submatrix of ^±_i which accounts for the rotation of (X^, Y^ , Z^) 
with respect to (Xj_]_, Y i _ 1 , Z.^). 


The Denavit-Hartenberg parameters which have been specifically assigned 
numerical values and those parameters which are carried symbolically in subsequent 
equations for assignment by researchers are shown in the table. In reference 2, ES 
WE, 03 = 90°, and ag = rg = 0. However, in the present paper, ES and WE may be 
different; and different constant values (refs. 2 and 5) for 03 and r3 can be 
used. Also, since preliminary measurements indicate that 03 may not be exactly 90°, 
a 3 is left unspecified in the equations. A method to calculate the 
Denavit-Hartenberg parameters for an assembled robot arm is developed in reference 
6. (The parameter r 2 would be chosen as zero in reference 6 because joints (2) and 
(3) produce parallel rotations; however, the nonzero value of r 2 used in reference 
2 (and here) can also be obtained by the same basic method in reference 6.) 


Notice in the table that is related to another joint angle 0^ 
(unprimed). The joint angles 0^ (i = 1,2,..., 6) are referenced to the initial 
position of the robot arm in figure 3. 

The transformation matrices A 1 ^ (in terms of 0*, i = 1,2,..., 6) are 
given in reference 2, except for a 3 2 , which is different because of the three 
unspecified parameters 03, r3, and a3» However, for convenience, all these 
transformation matrices are contained in appendix A. The rotational matrices 
^i-l are simply the 3x3 submatrices in the upper left-hand corner of 
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Resolved-Rate Control Equations 


Figure 4 shows the axis system (Xg, Yg, Zg) of the robot hand. (The hand 
itself is not shown.) With one three-axis controller, an operator commands trans- 
lational speeds TVEL(l), TVEL(2), and TVEL(3) along Xg, Yg, and Zg, respect- 
ively; and, with the other three-axis controller, he commands rotational speeds 
RVEL(l), RVEL ( 2 ) , and RVEL(3) about Xg, Yg, and Zg, respectively. That is, the 
operator commands the translational velocity 


TVEL 


’TVEL(l)] 
TVEL(2) I 
TVEL(3) J 


(5) 


and the rotational velocity 


RVEL 


'RVEL(l) 
RVEL(2) 
RVEL ( 3 ) 


( 6 ) 


xg, yg, and z£ are unit vectors along Xg, Yg, and Zg, respectively. 

These commands are then resolved by the computer into individual joint rotations in 
the robot arm to produce the commanded hand motion. 


The axis system for the robot hand may be located wherever desired for conven- 
ience; for example, near the tip of the robot hand (fig. 5) or, as in this paper, at 
the robot hand mounting (fig. l). In the sequel, commands of rotational and transla- 
tional velocity to the robot hand are expressed in terms of joint velocities. First, 
a word about notation: a vector is underlined and an overhead caret ( A ) indicates a 
vector expressed with respect to the base coordinate system (Xq, Yq, Zq). For 

example, is a unit vector along X^ but expressed in base coordinates. 

Rotational velocity of robot hand in base coordinates .- By convention, joint i in the 
robot arm rotates with angular speed 0'^ about Z^_^ (fig. 2). However, since 
0^ only differs from 0'^ by a constant offset, 0^ = 0'i* Thus, the 
rotational velocity of joint i is (fig. 6) 


oj. = 0 . z . (T) 

— i l—i-l 


or, with respect to the base-coordinate system. 


U). 
— i 



0 .z . , 
1 - 1-1 


( 8 ) 


The vector sum of these individual joint rotational velocities is the resultant rota- 
tional velocity of the robot hand: 

„ 6 . , . 

RVEL = E Rq 0 ± (9) 

i=l 
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In vector-matrix form, equation (9) is 


where 


A 

RVEL = 






i-1 


0 

0 

1 


( 10 ) 


( 11 ) 


is the unit vector Zj._i expressed in base coordinates and is simply the third 
column of Rg* - ! > an< ^ where 


0 


( 12 ) 



is a vector of joint velocities. (A programming notation in reference 2 is 
JVEL(i) = §i) 

Translational velocity of robot hand in base coordinates .- The resultant transla- 
tional velocity of the robot hand caused by individual joint relations in the robot 
arm is 


~ 6 

tvel = I v (13) 

i=l 


where joint i induces the translational velocity 


V. 
— 1 


0 .z . 

1—1-I 


—i-1, 6 


(14) 


which is the cross product of the joint rotational velocity (with respect to the base 
coordinate system) and the vector moment arm 


—i-1, 6 ^06 ^0,i-l 


(15) 


from the origin of coordinate system i-1 to the origin of the hand axis system. For 
example, the vector moment radii d 0 g and _dq6 are shown in figure 7. £ 0 k is the 
3x1 vector in the upper-right corner of A k 0 = A 1 qA 2^. . . that 

is, the first three entries in the fourth column of A k g. In vector-matrix form, 
equation (13) becomes 


TYEL = [zq x 



A A -» 

£5 x % 6 J | 


(16) 
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Jacobian matrix .- Equations (10) and (l6) are combined as 
TVEL 


RVEL 


= J 0 


using the Jacobian matrix 


J = 



“ ^ A 1 


J. -A A — 

- 

x 46 ' 
1 

4 x 46 | 

... , z_ 5 x d 56 


A 

I 

A 

1 

1 * 


1 

-i ! 

. . . , * 5 


Symbolically, for nonsingular J, 

A 

TVEL 


0 = J 


.-1 


RVEL 


(IT) 


(18) 


( 19 ) 


constitutes the set of joint angles which will produce commanded values of 

A A 

translational velocity TVEL and rotational velocity RVEL . For the robot arm in 
figure 1, there are six joint angles so that J is a 6x6 matrix. 

Control inputs in hand axis system .- In equations (17) and (19) the control inputs 
are with respect to the base coordinate system (as indicated by the overhead caret). 
However, in application, an operator watches the robot hand move and issues commands 
to the hand itself. Therefore, the auxiliary equation 


A “ 

TVEL 

_ 

R 

6 

_0 

TVEL 

RVEL 


R 

6 

0 

RVEL 


is needed to transform the operator’s inputs from the hand axis system to the 
base coordinate system. For specific elements in r6 0 see appendix B. Now, as 
indicated in figure 8, an operator inputs translational and rotational velocities in 
the hand axis system to make the robot arm move. These inputs are then transformed 
(eq. 20) to the base coordinate system for use in the resolved- rate equations (eqs. 
IT or 19) to compute the joint velocity (0) to drive the robot arm. These joint 
velocities must be integrated to obtain joint angles; for example, the arm moves to 
a new position Q^ew* which is related by Euler integration to its old position 
®01d the equation 


0, T = 0 O1 + 0 At 

-New —Old — 


( 21 ) 


where At is the time increment for computationally updating the joint angles in the 
robot arm. The operator varies his inputs to dynamically drive the robot arm by 
using feedback, such as visual, graphical, or force. 
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Location of Hand Axis System 


Simplification of J matrix .— In general, one is faced with solving equation (l7) for 

• /\ /\ 

_0, given TV EL and RVEL. Reduction in the computational complexity is beneficial for 
real-time operation. Toward this end, the origin of the hand coordinate system 
(Xg, Y 6 > z 6 ) i s chosen to coincide with the origins of (X 4 , Y 4 , Z 4 ) and 

(X 5 , Y 5 , Z 5 ) in figure 3 (ref. 2). Consequently, ^45 = dc>g = 0; and £3 

and d 3 g are parallel. Therefore, the three cross-product terms in the upper-right 
corner of J are zero, that is 


1 

X 

G\ 

I A A 1 

j 4 x i 

4 x ! ° 

I 

0 

! 0 

_ 

i 1 

1 



■ 

A 

a A 1 

A 1 A 

A 

1 A 

. ^0 

i > 

—2 ! —3 

^4 

i -5 


( 22 ) 


Equations for translational and rotational velocities of robot hand resulting from 
simplified J matrix .- From equations (17) and (22). 


TVEL 


( 4 x 46 )5 i + ( ^1 x V®2 + ( 4> x i 2 6 )5 : 


(23) 


—1® 2 ~ *2*3 = %°4 + - 54®5 + 

Hence, equation (23) is solved for 0 1} 0 2 , and 0 3 ; and, with these solutions, 
equation (24) is solved for 04, § 5 , and 0g. 


(24) 


Solving for Joint Rates §x, 0 2 , ©3 
Equation (23) can be expressed as (see appendix C) 


■ TVEL (1) " 


r- • " 

e i 

TVEL (2) 

= M 

• 

9 2 

A 



_ TVEL (3) - 

1 

9 3 J 


where 



-SI 

FI - 

Cl 

F2 

Cl F3 

Cl f4 “ 

M = 

Cl 

FI - 

SI 

F2 

SI F3 

Si f4 



0 



-FI 

-F5 


FI = S2 ES + F5 

F2 = SN + CAL3 WE 

F3 = C2 ES + F4 

F4 = C23 SAL3 WE - S23 A3 

F5 = S23 SAL3 WE + C23 A3 


(25) 


(2 6) 


(27) 

( 28 ) 

(29) 

(30) 

(31) 
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Singularities associated with M .- The determinant of M (appendix D), equated to 
zero, supplies the following singularity conditions: 


S2 ES + S23 SAL3 WE + C23 A3 = 0 
WE SAL3 S3 + A3 C3 = 0 


(32) 

(33) 


With variations in 0q (fig. 3), the origin of the hand axis system generates a 
circle about Zq. Equation (32) implies that the minimum radius for this circular 
motion has been reached; that is, no further motion normal to Zq is possible. For 
nominal values a3 = 0, ES = WE, and 03 = 90°, equation (32) reduces to the singu- 
lar condition S2 + S23 = 0 in reference 2. 

Equation (33) implies that the robot arm is at its maximum (or minimum) exten- 
sion with respect to the joint angle 63. For nominal values a3 = 0 and 
«3 = 90°, equation (33) corresponds to the singular condition 63 = 0 in reference 
2. (63 = l80° is not achievable with the robot arm depicted in figure 3.) With 

83 = 0 in figure 3, the robot hand can be extended no further along Zg. In 
figure 3, both singularity conditions are satisifed with all the translational 
velocity along and none along and Zp. 

Solution for nonsingular conditions (det (M) * 0) .- When not in a singular condi- 
tion, equation (25) is solved directly as 


0 = {(TVEL(l) SI - T VEL ( 2 ) Cl] [FI FU - F3 F5l }/det(M) 


(34) 


0 2 = -{F5[TVEL(1) FT + TVEL ( 2 ) F 6] + TVEL(3) FI F4}/det(M) (35) 


©2 = -Fl[TVEL(l) FT - TVEL ( 2 ) F6 + TVEL(3) F3 ] /det(M) 


(36) 


where 


FT = Cl FI - SI F2 


(3T) 


Generalized matrix inverse solution .- Near a singular condition (det (M) = 0), the 
generalized inverse matrix solution to equation (25) can be used rather than equa- 
tions (3*0 to (36). From equation (25), 


(38) 


where M* denotes the generalized inverse of M. For nonsingular M, M* = M“^. (in 
reference 2, expressions are generated for generalized matrix inverses corresponding 
to the singularity conditions S3 = 0 and S2 + S23 = 0; however, the double singular- 
ity which happens at S2 = S3 = 0 is not accounted for). 


• 

'V 


‘ TVEL (l)‘ 

• 

6 2 

= M* 

A 

TVEL (2) 

L e 3 J 


1 A 

.TVEL (3). 


To reduce the computational burden and benefit real-time operation, a further 
reduction in the matrix to be inverted is suggested. Multiply the first row of 
equation (25) by Cl, the second row by SI, and add the results to get 
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(39) 


®1 = ^ F3 ®2 + Fl » ® 3 ~ C1 TVEL(l) - SI TVEL(2)]/F2 

For the present robot arm in mind, CAL3”0 and SN*0 so that, from equation (28), 

F2*0. Therefore, given 0p and 63 , one computes 0p without difficulty in 
equation (39) • With equation (39) » the first and third rows of equation (25) can be 
written as 


gl = Ml 


where 


0 , 


F 6 = SI FI + Cl F2 
gl = 


F2 TVEL(l) - F6(C1 TVEL(l) + SI TVEL(2)) 

A 

TVEL ( 3 ) 


Ml = 


(Cl F2 - F6)F3 (Cl F2 - F 6 )FH 
-FI -F5 

Equation (4o) can be solved as 


] 


(40) 

(41) 
(U2) 

(43) 



= Ml* gl 


where Ml* is the generalized inverse of the 2x2 matrix Ml. 

• • • 

©2 and © 3 , one computes 0 ]_ with equation ( 39 )* 


(44) 


Then, with solutions for 


Variables held constant near singular points .- A method for avoiding singular points 
is to "freeze" variables when they come within a specified region of a singular 
point. A variable is held constant until a new value for the variable is computed 
outside the specified region. For example, if 02 and 03 approach the singular 
condition in equations (32) and ( 33 ) then the angles maintain their current values 
until new values are computed outside the singular region. This type of control 
excludes certain positions of the robot arm which may or may not be satisfactory, 
depending upon the task, and may also cause the arm to jerk. 


Auxiliary hand control .- People are limited in the speed with which they can move 
their arms and hands, and there are geometric constraints which disallow certain 
motions and positions. Yet, people have excellent control of their arms and hands 
without being consciously aware of these limitations, which can appear as singular- 
ities in mathematical equations. Therefore, it appears that some type of auxiliary 
hand control is warranted near singularities to help an operator gain additional 
control of the robot arm. Given a singular arm position, the auxiliary control 
scheme should take into consideration what the operator would most likely want to do 
in the given situation. Consider the following rudimentary scheme which may have 
some favorable characteristic motions for an operator. 


11 



In the vicinity of 63 =0, let 63 be proportional to the component of TVEL 
parallel to X 2 in figure 3. Thus, 




0 


A 


if 9 = 0 and (TVEL) > 0 


K3 (TVEL) v otherwise 
X 2 


( 45 ) 


where K3 is a constant to he specified. This means that 63 will not vary in 
response to an impossible outward (radial) motion command for the robot arm; whereas, 
if there is a component of velocity toward the shoulder of the arm, 83 will cause 
the arm to retract. Now, make 


0 2 = - [WE/ (WE + ES)] 6 3 + kJ( TVEL ) y /(WE + ES)] (46) 

where K 2 is a constant to be specified. The first term in equation (46) will cause 
©2 to vary so that the arm moves back in nearly a straight line from the hand 
towards the shoulder. The second term will allow the arm to pitch in the extended 

A 

position in proportion to the component of TVEL in the pitching direction for the 

A 

arm. The components of TVEL needed in equations (45) and (46) can be extracted in 
the process of computing the transformation from hand to base (appendix B). Equation 
( 39 ) specifies azimuth movement. 

Another feature which should be incorporated is the ability to bend the robot 
arm at the elbow in either the up or down direction. To do this, change the sign on 
K3 in equation (45) each time the arm enters the singularity mode. Consequently, an 
operator simply extends the arm and backs up again to reverse the directions of the 
elbow bend. This scheme has not yet been evaluated. There is the prospect that this 
" in-and-out” motion may be a desirable feature as the robot arm nears its maximum 
extension. Perhaps, this feature may be desirable in a larger region than just in a 
very small neighborhood of the singularity at the full extension of the robot arm. 
Whether or not the control superimposed by this scheme will be consistent with an 
operators desired control in singular situations remains to be seen from experiment. 


• • • 

Solving for Joint Rates 84 , 85 , 8 g 
Equation (24) can be written as 

RVEL = z^ + z^O + zX (47) 

where 

RVEL = RVEL - z^ - z^ - zX (48) 

is the resultant rotational velocity of that commanded and that induced hy the 
rotation of the first three joints. Equation (47) simplifies to the following three 

simultaneous equations in 04 , 05 , and 0g: (see appendix C) 

0 5 = -C4 D5 - S4 D4 (49) 
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(50) 


S5 0 6 = -S4 D5 + C4 D4 

6^ + C5 6g = SAL3 D3 - CAL3 D2 (5l) 

where 

D1 = Cl RVEL(l) + SI RVEL(2) (52) 

D2 = SI RVEL(l) - Cl RVEL(2) (53) 

D3 = S23 D1 + C23 RVEL(3) (54) 

D4 = C23 D1 - S23 RVEL(3) (55) 

D5 = CAL3 D3 + SAL3 D2 (56) 


Equation (49) clearly shows that there is no difficulty in computing 0^; and 

• • 

equations (50) and (51) show that the only difficulty in computing 0g and then 64 
is when 0^ = 0. In this case the mathematics cannot decide which angle to vary to 
produce a rotation about Zg (fig. 3). Some type of maximum angular rate penality 
may be needed to avoid excessive rates near singular points; that is, when S5“0 in 
equation (50). 

Generalized matrix inverse Write equations (50) and (5l) as 


where 


Then, 



M2 = 


g2 = 


0 S5 
. 1 C5 
-S4 d 6 + c4 d4 
. SAL3 D3 - CAL3 D 2 . 



= M2* g2 


(57) 


(58) 

(59) 


( 60 ) 


where M2* is the generalized inverse of the 2x2 matrix M2. Again, ©5 is computed 
with equation (49). 


A total solution for 0* (i = 1, 2, . . . , 6) is provided by equations (39), (44), 
(49), and (60). Hence, only the generalized inverses of two 2x2 matrices are 
required. For some kinematically redundant manipulators, control based on general- 
ized matrix inverses can lead to undesirable arm configurations (ref. 7). 
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Choose which angle to rotate near singularity .- When 85 = 0, equation (51) becomes 

6^ + 6g = SAL3 D3 - CAL3 D2 ( 6l ) 

and equation ( 50 ) is useless. The generalized matrix inverse solution actually 
splits the rotational task equally between 64 and 6g when 85 = 0. Another 

approach is to make 84 = 0 when 85 is less than a prescribed amount and let 


9g = SAL3 D3 - CAL3 D2 


(6 2 ) 


Then, for additional rotational capability when 0 g reaches a limit, continue rota- 
tion with 


0^ = SAL3 D3 - CAL3 D2 


( 63 ) 


Uncoupled arm and wrist motions .- An approach to avoid the wrist singularity that 

occurs when 85 = 0 is to: (l) position the wrist with 0]_, 89, and 83 (eq. 

23 ); and (2) individually command the joint velocities 84, 0c, and 0g at the 
wrist of the robot arm. 


DISCUSSION 


Visual observation of robot hand orientation .- In implementing resolved- rate control 
on an industrial robot manipulator with a symmetrical hand, the following annoyances 
were observed: 

1 . Keeping track of the positive hand axes: (Painting the hand might help 

alleviate this problem.) 

2 . Keeping track of wrist orientation angles to avoid hitting limits. 
(Displaying the angles to an operator will help, but this only increases his 
workload. Perhaps, a better solution is to add appropriate hierarchical 
control to handle this "out-of-range" problem for the operator.) 

Large ang ular rates .— Another nuisance experienced in the teleoperator control of a 
robot arm is the occurrence of large angular rates near mathematical singularities. 
Hopefully, a hierarchical control structure on top of the operator r s control will 
eliminate this problem. 

Different designs of robot arm .— Other robot arm designs may offer certain control 
advantages. 


Ability to record trajectory segment .- Current robot manipulators can be "taught" 
a trajectory to be repeated later upon command. Hence, a recent trajectory segment 
can be reversed to provide an operator with the option of a speedy and effortless 
evacuation of the robot hand from a congested workspace. 

It appears that some type of auxiliary hand control structure would be useful 
in aiding a human operator (or computer) in the control of a robot arm. For 
instance, such control should be formulated to produce the motions that are probably 
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what an operator would like to do in a current predicament and to allow alternatives 
based on his responding inputs. Of course, in future path planning with constraints, 
hierarchical control would be of additional assistance to the operator. 

CONCLUDING REMARKS 


Kinematic equations for resolved-rate control of a robot arm are simplified to 
allow faster computations, and control in singular regions is discussed. 
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APPENDIX A 


HOMOGENEOUS TRANSFORMATION MATRICES 


When the parameters in the table are introduced into the general transformation 
matrix (eq. l), the following six transformation matrices result. These matrices are 
the same as those used in reference 2, except for notation and which has 

three additional unspecified parameters a 3 , r 3 , and 013. In the following 
matrices, A3 = a 3 , R3 = r 3 , SAL3 = sina 3 , CAL3 = cosa 3 . Cl = cos0]_, SI = 
sinOi, etc. 


-Cl 

-SI 

0 

0 


0 

0 

1 

0 


-SI 

Cl 

0 

0 


0 

0 

NO 

1 


-S2 

C2 

0 

0 


-C2 

-S2 

0 

0 


0 

0 

1 

0 


-ES S2 
ES C2 
SN 
1 


-S3 

C3 

0 

0 


-C3 CAL3 
-S3 CAL3 
SAL3 
0 


C3 SAL3 
S3 SAL3 
CAL3 
0 


-A3 S3 
A3 C3 
R3 
1 


-C4 

-s4 

0 

0 


0 

0 

1 
0 


-S4 

C4 

0 

0 


0 

0 

WE 

1 


-C5 

-S5 

0 

0 


0 

0 

1 

0 


-S5 

C5 

0 

0 


0 

0 

0 

1 


c6 

S6 

0 

0 


-S 6 

c6 

0 

0 


0 

0 

1 

0 


0 

0 

HW 

1 


(Al) 


(A2) 


(A3) 


(a4) 


(A5) 


(a 6 ) 
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APPENDIX B 


TRANSFORMATION FROM HAND TO BASE (THTOB) 


Operator inputs for controlling the robot arm are resolved in directions 
parallel to the base-coordinate axes by 


A 


r 6 


TVEL 


R o 

TVEL 

A 


6 


RVEL 


R o 

RVEL 


(Bl) 


where the rotational matrix can be associatively grouped into products of matrices as 


where 


R o - K (r ^> H 

R 3< R U R 5>] 


(B2) 

R 3< e J R 5> - 

" Q1 
Q2 

_ -S5 C6 

Q3 

Q4 

S5 S6 

C4 S5 " 
S4 S5 
C5 

(B3) 

2 3 

R 1 R 2 = 

" Pi 
P2 
0 

-P2 CAL3 
PI CAL3 
SAL3 

P2 SAL 3 " 
-PI SAL 3 
CAL3 

(B4) 


where 


Ri(R?Ro) = 


0 12 


-Cl PI Cl P2 CA3 - SI SAL3 -Cl P2 SAL3 - SI CAL3 
-SI PI SI P2 CA3 + Cl SAL3 -SI P2 SAL3 + Cl CAL3 
P2 PI CAL 3 -PI SAL 3 


(B5) 


Qi = c4 C5 c 6 - s4 s 6 

(B6) 

Q2 = S4 C5 C 6 + c4 s6 

(BT) 

Q3 = -C4 C5 S 6 - s4 c6 

(B8) 

Q4 = -S4 C5 S6 + c4 C 6 

(B9) 

PI = -C23 

(BIO) 

P2 = -S23 CAL 3 

(Bll) 


Multiplying equation (B3) on the left by equation (B5) produces a matrix L 
(identical to R°q)« Let 


T1 = Cl CAL3 P2 - SI SAL3 
T2 = -Cl P2 SAL3 - SI CAL3 
T3 = SI CAL3 P2 + Cl SAL3 
T4 = -SI P2 SAL3 + Cl CAL3 


(B12) 

(B13) 

(Blit) 

(B15) 


IT 



Then the elements of L are: 


L(l,l) = -Cl PI Q1 + T1 Q2 - T2 S5 C6 

L(2,l) = -SI PI Q1 + T3 Q2 - TU S5 C6' 

L(3,l) = P2 Q1 + PI CAL3 Q2 + PI SAL3 S5 C6 

L(l,2) = -Cl PI Q3 + T1 Q4 + T2 S5 S 6 

L(2,2) = -SI PI Q3 + T3 QU + TU S5 S 6 

L(3,2) = P2 Q3 + PI CAL3 QU - PI SAL3 S5 S6 

L(l,3) = -Cl PI C4 S5 + T1 SU S5 + T2 C5 

L(2,3) = -SI PI Ch S5 + T3 Sh S5 + TU C5 

L(3,3) = P2 CU S5 + PI CAL3 Sk S5 - PI SAL3 C5 


(B16) 

(BIT) 

(B18) 

(B19) 

(B20) 

(B21) 

(B22) 

(B23) 

(B2U) 


The translational and rotational velocities of the robot hand are then resolved 
in directions parallel to the base coordinate system by equation (Bl), with 
r 6 0 = L. 

The rotational transformation matrices (R^-i.i) are the 3x3 submatrices in 

the upper left-hand corner of the homogeneous transformation matrices in 

appendix A. The inverses of these rotational matrices are also their transposes. 
Thus , 


(B25) 


*?• 

-Cl 

-SI 

0 

0 

0 

1 


-SI 

Cl 

0 


"-S2 

C2 

0 

R 2 = 

-C2 

-S2 

0 


0 

0 

1 

A , , 

- 

A 

- 

TVEL(2) 

, and 

TVEL 

(3) a 


(B26) 


A 




A 


^Z 2 be the components of TVEL relative to the coordinate system (X 2 , Y 2 , 
Z 2 ). Then, these components are related by the transformation equation. 


(tvel) y 


A 

TVEL ( 1 ) " 

A 2 



(TVEL) 

2 

- R 1 R° 
R 2 R 1 

A 

TVEL(2) 

(tvel)„ 
z 2 J 


TVEL ( 3 ) 


(B27) 


A 

Therefore, in the text, the components of TVEL in equation (1*5) and (46) are as 
follows: 

( TVEL ) X = S2 [ci TVEL(l) - SI TVEL(2) + C2 TVEL(3)] (B28) 


^ A A A 

(TVEL) y = C 2 [ci TVEL(l) - SI TVEL(2) - S2 TVEL(3)] 


(B29) 
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APPENDIX C 


RESOLVED-RATE EQUATIONS 

Joint rates are computed by solving equations (23) and (47) for commanded 
velocities of the robot hand. These equations, repeated here for convenience, 

^ =( ^0 X *06 )§ 1 + ( ^-l x *l6 } ®2 + ( — 2 X — 26 } ®3 


are 


and 


where 


RVEL = z_0, + z, 0_ + z _0 

—3 4 —4 5 —5 6 


RVEL = RVEL - z {) 0 1 - z x 0 2 - Zg0 


is the resultant rotational velocity of that commanded and that induced by 
rotations of joints 1, 2, and 3. 

The third column of R k g is z^, which is easily seen from 




A 


0 

0 

1 


Equivalently, z^ is the third column of 


A k .1.2 

A 0 A 0 A 1 * * 


4- 1 


if the fourth element in the third column is disregarded. 
The identity homogeneous transformation matrix is 
1 
0 
0 
0 


$ 


(Cl) 


(C2) 


(C3) 


(C4) 


(C5) 


0 

0 

1 

1 

0 ~ 


1 

0 

1 

1 

1 

0 

(C 6) 

0 

1 

1 

1 

0 


0 

0 

— 1 — 

1 

1 

1 



From equation (C6), 


^0 = 


0' 

0 

1 


(CT) 


From equation (Al) in appendix A, 
-Si' 


*1 = 


Cl 

L 0 J 


(C8) 
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The third column (ignoring the fourth element) of the product 


^0 



Cl S2 

Cl C2 

-SI 

1 

1 

1 

ES 

Cl 

S2 - 

SN 

SI 

= aV = 
A 0 A 1 

SI S2 

SI C2 

Cl 

1 

1 

1 

1 

ES 

SI 

S2 + 

SN 

Cl 


°2 

-S2 

0 

1 

1 

1 

1 

ES 

C2 

+ NO 




0 

0 

0 

'"t" 

1 

1 

1 


1 





yields 


—2 


-SI 

Cl 

0 


(C9) 


(CIO) 


In a similar manner. 


—3 


Cl S23 SAL3 - SI CAL3 
SI S23 SAL3 + Cl CAL3 
C23 SAL3 


^4 " 


-Cl C23 S4 - Cl S23 CAL3 C4 - SI SAL3 C4 
-SI C23 S4 - SI S23 CAL 3 C4 + Cl SAL3 C4 
S23 S4 - C23 CAL3 C4 


^5 


Cl C23 C4 S5 - Cl S23 CAL3 S4 S5 - SI SAL3 S4 S5 
+ Cl S23 SAL3 C5 - SI CAL3 C5 

SI C23 C4 S5 - SI S23 CAL3 S4 S5 + Cl SAL3 S4 S5 
+ SI S23 SAL3 C5 + Cl CAL3 C5 

-S23 C4 S5 - C23 CAL3 S4 S5 + C23 SAL3 C5 


(Cll) 


(C12) 


(C13) 


The fourth column A^q (again, exclude the fourth element) provides jDQk’ a 
vector in base coordinates from the base-coordinate system to coordinate system k. 
From equation (Al) in appendix A, 


£oi = 



(Cl4 ) 
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From equation (C9), 


£q2 


In a similar manner. 


£03 


ES Cl S2 - SN SI 
ES SI S2 + SN Cl 
ES C2 + NO 

Cl C23 A3 + Cl ES S2 - SI SN 
SI C23 A3 + SI ES S2 + Cl SN 
-S23 A3 + ES C2 + NO 


£o4 


Cl S23 SAL3 WE - SI CAL3 WE + Cl C23 A3 + Cl ES S2 - SI SN 
SI S23 SAL3 WE + Cl CAL3 WE + SI C23 A3 + SI ES S2 + Cl SN 
C23 SAL3 WE - S23 A3 + ES C2 + NO 


£05 


where 


Cl S23 SAL3 WE - SI CAL3 WE + Cl C23 A3 + Cl ES S2 - SI SN " 
SI S23 SAL3 WE + Cl CAL3 WE + SI C23 A3 + SI ES S2 + Cl SN 
C23 SAL 3 WE - S23 A3 + ES C2 + NO 


SN = SN + R3 


Since coordinate systems 5 and 6 coincide. 


(C15) 


(Cl6) 


(C1T) 


(C18) 


(C19) 


£06 £05 


(C20 ) 


A vector (in base coordinates) 
system is 


from coordinate system k to the robot hand-axis 


— k6 £06 “ £0k 


Hence, 

' Cl S23 SAL3 WE - SI CAL3 WE + Cl C23 A3 " 
+ Cl ES S2 - SI SN 

SI S23 SAL3 WE + Cl CAL3 WE + SI C23 A3 
d. = £ = + SI ES S2 + Cl SN 

06 06 

C23 SAL3 WE - S23 A3 + ES C2 + NO 


(C21 ) 


(C22) 


(£ = 0) 
00 
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46 ~ 4)6 ~ 4i 


Cl S23 SAL 3 WE - SI CAL3 WE + Cl C23 A3 
+ Cl ES S2 - SI SN 

SI S23 SAL3 WE + Cl CAL3 WE + SI C23 A3 
+ SI ES S2 + Cl SN 


C23 SAL3 WE - S23 A3 + ES C2 


-26 4)6 ~ 4)2 


Cl S23 SAL3 WE - SI CAL3 WE + Cl C23 A 3 
- SI R3 

SI S23 SAL 3 WE + Cl CAL 3 WE + SI C23 A3 
+ Cl R3 


-36 46 " 4)3 


C23 SAL3 WE - S23 A3 


Cl S23 SAL 3 WE - SI CAL3 WE 
SI S23 SAL3 WE + Cl CAL3 WE 
C23 SAL3 WE 


(C25) 


By design, coordinate systems U, 5, and 6 coincide so that d)^ = £56 = dgg = 0. 

Translational velocity equations .- The required cross products in equation (Cl) are: 

f-Sl S23 SAL3 WE - Cl CAL3 WE - SI C23 A3 1 
-SI ES S2 - Cl SN 


4 X 46 


Cl S23 SAL3 WE - SI CAL3 WE + Cl C23 A3 
+ Cl ES S2 - SI SN 


(C26) 


4 X 46 


C1(ES C2 + SAL3 C23 WE) - Cl S23 A3 
S1(ES C2 + SAL3 C23 WE) - SI S23 A3 
-SAL3 WE S23 - ES S2 - C23 A3 


(C27) 


4 x 46 


Cl WE C23 SAL3 - Cl S23 A3 
SI WE C23 SAL3 - SI S23 A3 
-S23 SAL3 WE - C23 A3 


(C28) 
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With equations (C2 6 ), (C27), and (C28), equation (Cl) can be expressed 


as 


"TVEL (1)~ 


A 

TVEL (2) 

= 

A 

.TVEL (3) _ 



-SI FI - Cl F2 Cl F3 Cl Fit 

Cl FI - SI F2 SI F3 SI Fit 


0 


-FI 


-F5 


u ^ J 


where 


(C29) 


FI = S2 ES + F5 

F2 = SN + CAL3 WE 

F3 = C2 ES + Fit 

Fit = C23 SAL3 WE - S23 A3 

F5 = S23 SAL3 WE + C23 A3 


(C30) 

(C31) 

(C32) 

(C33) 

(C3lt) 


Rotational velocity equation.- With equations (Cll), (C12), and (C13), equation (C2) 
becomes 


‘ RVEL (1) " 


RVEL (2) 

= 

.RVEL (3) . 



Cl S23 SAL3 - SI CAL3 
SI S23 SAL 3 + Cl CAL3 
L C23 SAL3 


-C1(C23 Sit + c4 S23 CAL3) 

- SI C4 SAL3 

-SI (C23 S4 + C4 S23 CAL3) 
+ Cl C4 SAL3 

S23 S4 - C23 C4 CAL3 


(Cl C23 C4 - SI S4 SAL3 - Cl S23 S4 CAL3)S5 + (Cl S23 SAL3" 
- SI CAL3)C5 

(SI C23 C4 + Cl S4 SAL3 - SI S23 S4 CAL3)S5 + (SI S23 SAL3 
+ Cl CAL3)C5 

-(S23 C4 + C23 S4 CAL3)S5 + C23 SAL3 C5 



(C35) 


Equation (C35) is now simplified considerably as follows. Multiply the first row 
of equation (C35) by Cl, the second row by SI, and add the results to get 


D1 = S23 SAL3 0^ - (C23 S4 + C4 S23 CAL3)0 5 

+[(C23 C4 - S23 S4 CAL3)S5 + S23 SAL3 C5 ] 0^ 


(C36) 


where 


D1 = Cl RVEL (1) + SI RVEL (2) 


(C37) 
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Now, multiply equation (C36) by S23, row three of equation (C35) by C23, and add to 
get 


D3 = SAL3(e 4 + C5 0g) - Cj^CAL3 0 ? - S4 CAL3 S5 0g (C38) 

where 

D3 = S23 D1 + C23 RVEL(3) (C39) 

Multiply row 1 in equation (C35) by SI and subtract Cl times row 2 to obtain 

D2 = -CAL3(0 U + C5 0g) - C4 SAL3 - S4 SAL3 S5 0g (C40) 


where 


D2 = SI RVEL(l) - Cl RVEL ( 2 ) (c4l) 

Multiply equation (C36) by C23 and subtract S23 times row 3 of equation (C35) to 
produce 


D4 = -S4 0 5 + C4 S5 0g 


(C42) 


where 


D4 = C23 D1 - S23 RVEL(3) (c43) 

Multiply equation (C38) by CAL3 and add to SAL3 times equation (c4o) to get 

D5 = -C4 9 5 - S4 S5 0 g (c44) 


where 


D5 = CAL3 D3 + SAL3 D2 (C45) 

Finally, multiply equation (C44) by -C4 and add to -S4 times equation (C42) to obtain 

0 5 = -C4 D5 - S4 d4 ( C 46) 

Then, multiply equation (c44) by -S4 and add C4 times equation (c42) to obtain 

S5 0g = -S4 D5 + C4 D4 (C47) 

The end result is obtained by multiplying equation (C38) by SAL3 and subtracting 
CAL3 times equation (C40). Thus, 

+ C5 Qg = SAL3 D3 - CAL3 D2 (C48) 

Therefore, equation (C35) has been simplified to the three scalar equations (C46), 
(C4T), and (C48). 
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APPENDIX D 


DETERMINANT OF M 


In this appendix, the determinant of M (eq. (26)) is expanded explicitly in 
terms of the joint angles and robot arm parameters. The determinant 


det(M) = det 


-SI FI - Cl F2 Cl F3 Cl FU 

Cl FI - SI F2 SI F3 SI FU 

0 -FI -F5 J 


expands to 

det (M) = Fl[F5(F3 - FU) - FU S2 ES) 

But, from equations (28) and (29), 

F3 - F4 = C2 ES 

Therefore , 

det(M) = FI ES(F5 C2 - Fk S2) 

From equations (30) and (31), 

F5 C2 - F4 S2 = SAL3 WE(S23 C2 - C23 S2) 
+ A3(C23 C2 + S23 S2) 

which, with the trigonometric identities 

S23 C2 - C23 S2 = S3 

C23 C2 + S23 S2 = C3 

becomes 


(Dl) 


(D2) 

(D3) 

(DU) 

(D5) 

(d6) 

(D7) 


F5 C2 - FU S2 = SAL3 WE S3 + A3 C3 (D8) 

With equations (D8) and equation (27) for FI in the text, the determinant of M in 
equation (DU) is expressed simply as 

det(M) = (S2 ES + F5)(SAL3 WE S3 + A3 C3)ES (D9) 
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Figure 2.- Joint axis systems defined "by Denavit-Hartenberg 
parameters a^ ol, r^, and 0^. 
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Figure 3. Initial position of robot arm, joint axis systems, 
and commanded robot hand velocities. 
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Figure 4.- Robot hand translational and rotational 
velocity components . 
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Figure 5-- Robot hand axis system. 
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Figure 7.- Illustration of moment radius vectors d , and d ,, 

—06 —16 


needed in calculating contributions to translational 
velocity of robot hand caused by rotations of joints 
1 and 2 . 
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Figurs ®*“ Illustration of information flow in rotot arm control. 
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